﻿#!/usr/bin/env python
# -*- coding: utf-8 -*-
# -*- Python -*-

from __future__ import print_function
import tkjoystick
import math
import OpenRTM_aist
import RTC
import sys
import time
sys.path.append(".")

# Import RTM module

# for convert()

# This module's spesification
# <rtc-template block="module_spec">
tkjoystick_spec = ["implementation_id", "TkJoyStick",
                   "type_name", "TkJoyStick",
                   "description", "Sample component for MobileRobotCanvas component",
                   "version", "1.0",
                   "vendor", "Noriaki Ando and Shinji Kurihara",
                   "category", "example",
                   "activity_type", "DataFlowComponent",
                   "max_instance", "10",
                   "language", "Python",
                   "lang_type", "SCRIPT",
                   ""]
# </rtc-template>


class Position:
    def __init__(self, x=0.0, y=0.0, r=0.0, th=0.0):
        self.x = x
        self.y = y
        self.r = r
        self.th = th


position = Position()
#tkJoyCanvas = tkjoystick.TkJoystick()


class TkJoyStick(OpenRTM_aist.DataFlowComponentBase):
    def __init__(self, manager):
        OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)

        self._k = 1.0
        self.x = 0.0
        self.y = 0.0

    def onInitialize(self):
        self._d_pos = RTC.TimedFloatSeq(RTC.Time(0, 0), [])
        self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
        self._d_vel = RTC.TimedFloatSeq(RTC.Time(0, 0), [])
        self._velOut = OpenRTM_aist.OutPort("vel", self._d_vel)

        # Set OutPort buffers
        self.addOutPort("pos", self._posOut)
        self.addOutPort("vel", self._velOut)

        return RTC.RTC_OK

    def onShutdown(self, ec_id):
        return RTC.RTC_OK

    def onExecute(self, ec_id):
        self._d_pos.data = [self.x, self.y]
        self._d_vel.data = self.convert(self.x, self.y)
        self._posOut.write()
        self._velOut.write()

        return RTC.RTC_OK

    """
   \brief Converting from canvas data to MobileRobotCanvas data
  """

    def convert(self, x, y):
        _th = math.atan2(y, x)
        _v = self._k * math.hypot(x, y)
        _vl = _v * math.cos(_th - (math.pi / 4.0))
        _vr = _v * math.sin(_th - (math.pi / 4.0))
        print(x, y, _vl, _vr)
        return [_vl, _vr]

    def set_pos(self, pos, pol):
        self.x = pos[0]
        self.y = pos[1]
        self.r = pol[0]
        self.th = pol[1]


# def MyModuleInit(manager):
#    profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
#    manager.registerFactory(profile,
#                            TkJoyStick,
#                            OpenRTM_aist.Delete)
#
#    # Create a component
#    comp = manager.createComponent("TkJoyStick")


def main():
    tkJoyCanvas = tkjoystick.TkJoystick()
    tkJoyCanvas.master.title("TkJoystick")
    mgr = OpenRTM_aist.Manager.init(sys.argv)
    mgr.activateManager()

    # Register component
    profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
    mgr.registerFactory(profile,
                        TkJoyStick,
                        OpenRTM_aist.Delete)
    # Create a component
    comp = mgr.createComponent("TkJoyStick")

    tkJoyCanvas.set_on_update(comp.set_pos)
    mgr.runManager(True)
    tkJoyCanvas.mainloop()


if __name__ == "__main__":
    main()
